#include "user.h"

//位置式pid参数初始化
void PID_init(PID_ABS_t *pid, float Kp, float Ki, float Kd, float Max_error, float Max_output)
{
	pid->Kp = Kp;
	pid->Ki = Ki;
	pid->Kd = Kd;
	pid->Max_error = Max_error;
	pid->Max_output = Max_output;
}
//位置式pid算法
void PID_Calc(PID_ABS_t *pid, float current_parament, float target_parament)
{
	pid->error[0] = pid->error[1];
	pid->error[1] = target_parament - current_parament;
	pid->error[2] += pid->error[1];

	if (pid->error[2] > pid->Max_error)
		pid->error[2] = pid->Max_error;
	else if (pid->error[2] < -pid->Max_error)
		pid->error[2] = -pid->Max_error;

	pid->Output = pid->Kp * pid->error[1] + pid->Ki * pid->error[2] + pid->Kd * (pid->error[1] - pid->error[0]);

	if (pid->Output >= pid->Max_output)
		pid->Output = pid->Max_output;
	else if (pid->Output <= -pid->Max_output)
		pid->Output = -pid->Max_output;
}

#define FPS 5          //摄像头帧率
#define T_Finish  0.2f  //云台收敛时间

float Camera_PID_Position(float new_inc,float error_v[],float inc[])
{
	static float error =0;
		
	/*更新数组*/	
	inc[1]=new_inc;
	error_v[0]=error_v[1];
	error_v[1]=inc[1]-inc[0];
	inc[0]=inc[1];
	
	/*线性拟合*/
	error = inc[1]																		 //零阶
					+ error_v[1]*T_Finish*FPS;              									 //一阶
//					+ (error_v[1] - error_v[0])*0.5f*T_Finish*T_Finish*FPS;    //二阶
	
	return error;
} 
